﻿//*************************************************************************
//面向服务的协作式智能交通系统仿真软件SoC-ITSS v2.1 alpha
//发布单位：西北工业大学--巴黎高科MINES机器人与群智能系统联合实验室
//团队负责人：张凯龙 Zhang Kailong
//团队成员：张大方、杨安胜、王敏、谢策、王雨佳、谢尘玉等
//联系我们：kl.zhang@nwpu.edu.cn
//2012-2021
//*************************************************************************

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Diagnostics;
using CyberCarSimulation.Regulation;
using CyberCarSimulation.Bean;
using CyberCarSimulation.RoadModel;
using CyberCarSimulation.Algorithm;

//*******************************************************
//
//文件名(File Name):           Simulation.cs
//
//数据表(Tables):              nothing 
//
//          修改理由：         仿真控制程序
//
//*******************************************************

namespace CyberCarSimulation.Simulation
{
    class Simulation
    {

        private static readonly float LeastInterval = 1.0f;     //最短间隙时间

        ///*存储原车道的车辆信息*/
        //private Queue<Vehicle> WtoE_Lane = null;
        //private Queue<Vehicle> EtoW_Lane = null;
        //private Queue<Vehicle> NtoS_Lane = null;
        //private Queue<Vehicle> StoN_Lane = null;        


        private Random Rand = new Random();     //产生随机数
        private float Lamda;                    //泊松分布的参数
        private float PecentageBig = 0.0f;  //大车比例
        private int Bnum = 0;    //大车数量
        private int Snum = 0;    //小车数量
        private int Prob_High;   //高优先级车辆数量             
        private int Prob_Mid;    //中优先级车辆数量  
        private int Prob_Low;    //低优先级车辆数量  
        private int Count;       //每条道路上的车辆数量

        public static int RenewCount = 0;   //发送的Renew消息数量
        public static float SCS_Length = 3.5f;  //车道宽度
        private static float LeastTimeInterval = 2.0f;  //产生车辆的最小时间

        /*当前车行驶一定距离后才能再插入新的车辆，主要是考虑到拥塞严重的话后车可能没有足够的刹车距离*/
        private double LeastDistance = Vehicle.MAX_SPEED_TAIL_SAFE_DISTANCE + Definitions.Small_Length;

        public Simulation(float Lamda, int Prob_High, int Prob_Mid, int Count, float PercentageBig)
        {
            this.Lamda = Lamda;
            this.Prob_High = Prob_High;
            this.Prob_Mid = Prob_Mid;
            this.Prob_Low = MainData.SimulationConfig.GetTxtVehicleQuantity() - Prob_High - Prob_Mid;

            //this.Prob_Low = 100 - this.Prob_High - Prob_Mid;
            this.Count = Count;
            this.PecentageBig = PercentageBig;
            this.Bnum = (int)(this.Count * PercentageBig);
            this.Snum = this.Count - this.Bnum;                  
        }

        /************************************************************************/
        /* 对外的函数接口，用于初始化每个车道队列                                                                     */
        /************************************************************************/
        public void initVehicleQueue()
        {
            //遍历进入车道，生成车流，将车流放入Queue
            foreach (Lane L in MainData.EnterLaneList)
            {
                //Test
                //if (L.GetID().Equals("4040511") || L.GetID().Equals("4010531") || L.GetID().Equals("4020631") || L.GetID().Equals("4030731"))
                //    if (L.GetID().Equals("4040511") || L.GetID().Equals("4010531"))
                //        if (L.GetID().Equals("4040511") || L.GetID().Equals("4020631") || L.GetID().Equals("4010531") || L.GetID().Equals("4091011")
                //            || L.GetID().Equals("4030731"))





                if (L.GetID().Equals("4010531") || L.GetID().Equals("4020631") || L.GetID().Equals("4030731")
                    || L.GetID().Equals("4040511") || L.GetID().Equals("4080721")
                    || L.GetID().Equals("4091011") || L.GetID().Equals("4131221")
                    || L.GetID().Equals("4141041") || L.GetID().Equals("4151141") || L.GetID().Equals("4161241"))

               
                //if (L.GetID().Equals("4040511"))
                {
                    Queue<Vehicle> VehicleQueue = generateVehicleQueue1(L.GetID(), MainData.SimulationConfig.GetTxtVehicleQuantity());
                    L.SetVehicleQueue(VehicleQueue);
                }
                else 
                {
                    Queue<Vehicle> VehicleQueue = new Queue<Vehicle>();
                    L.SetVehicleQueue(VehicleQueue);
                }


                //车道ID  车辆数目                
                //Queue<Vehicle> VehicleQueue = generateVehicleQueue1(L.GetID(), MainData.SimulationConfig.GetTxtVehicleQuantity());
                //L.SetVehicleQueue(VehicleQueue);

                //离开车辆队列  PassedList
                List<string> PassedList = new List<string>();
                L.SetPassedList(PassedList);

                //存放当前所在车道Lane
                List<Vehicle> VehicleList = new List<Vehicle>();
                L.SetVehicleList(VehicleList);
                             
            }

            //遍历离开车道，初始化车辆Queue
            foreach (Lane L in MainData.LeaveLaneList)
            {
                Queue<Vehicle> VehicleQueue = new Queue<Vehicle>();
                L.SetVehicleQueue(VehicleQueue);

                //离开车辆队列 PassedList
                List<string> PassedList = new List<string>();
                L.SetPassedList(PassedList);

                //存放当前所在车道Lane
                List<Vehicle> VehicleList = new List<Vehicle>();
                L.SetVehicleList(VehicleList);
            }

            //遍历中间车都，初始化车辆Queue
            foreach (Lane L in MainData.CenterLaneList)
            {
                Queue<Vehicle> VehicleQueue = new Queue<Vehicle>();
                L.SetVehicleQueue(VehicleQueue);

                //离开车辆队列 PassedList
                List<string> PassedList = new List<string>();
                L.SetPassedList(PassedList);

                //存放当前所在车道Lane
                List<Vehicle> VehicleList = new List<Vehicle>();
                L.SetVehicleList(VehicleList);
            } 
        }

        /************************************************************************/
        /* 在指定的车道内随机生成指定数量的车辆               
         * LaneID 车道的ID
         * Count 生成车辆的数量
         * List<Vehicle>将生成的车辆放到List中返回
        /************************************************************************/
        private Queue<Vehicle> generateVehicleQueue(string LaneID, int Count)
        {
            Queue<Vehicle> VehicleQueue = new Queue<Vehicle>();
            int index = 0;        //车辆数目
            string DLaneID;       //标识目标车道
            int prio = -1;        //标识当前车的优先级
            int prob = -1;        //随机数的产生概率
            float ttime = 0.0f;   //原来时间
            float gtime;          //最新时间
            string CarID;         //车辆ID
            int Hcnt = 0;         //高优先级车数量
            int Mcnt = 0;         //中优先级车数量
            int Bcnt = 0;         //大车数量
            int CarType = Definitions.SmallCar; //标志车辆类型

            while (index < Count)
            {
                /*车辆生成时间*/
                gtime = this.getRandomInterval(this.Lamda);
                gtime += ttime;  //迭代产生时间 新时间
                ttime = gtime;   //迭代产生时间 保留原时间


                /*随机产生车辆的目标车道Destination_LaneID*/
                DLaneID = this.SetDstLaneID(LaneID);


                /*优先级设置*/
                bool mark = true;      //用于标识高中优先级车辆是否超过预计数量，若超过则为true，否则为false
                while (mark)
                {
                    mark = false;
                    /*优先级位置随机生成*/
                    prob = (this.Rand.Next(1000) % MainData.SimulationConfig.GetTxtVehicleQuantity()) + 1;
                    if (prob > 50)
                    {
                        Debug.WriteLine("fdsafafs35555555");
                        Debug.WriteLine("fdsafafs111111");
                    }
                    if (0 == prob)
                    {
                        Debug.WriteLine("fdsafafs111111");
                        Debug.WriteLine("fdsafafs111111");
                    }
                    if (1 <= prob && prob <= this.Prob_High)//高优先级
                    {
                        if (Hcnt >= this.Prob_High)
                        {
                            /*超出预期的范围*/
                            Debug.WriteLine("fdsafafs33333333");
                            Debug.WriteLine("fdsafafs111111");
                            mark = true;
                            continue;
                        }
                        else
                        {
                            prio = Definitions.Prio_High;
                            Hcnt++;
                        }
                    }
                    else if (this.Prob_Mid != 0 && prob > this.Prob_High && prob <= (this.Prob_High + this.Prob_Mid))//中优先级
                    {
                        if (Mcnt >= this.Prob_Mid)
                        {
                            Debug.WriteLine("fdsafafs444444444");
                            Debug.WriteLine("fdsafafs111111");
                            mark = true;
                            continue;
                        }
                        else
                        {
                            prio = Definitions.Prio_Mid;
                            Debug.WriteLine("fdsafafs111111");
                            Mcnt++;
                        }
                    }
                    else
                    {
                        prio = Definitions.Prio_Low;
                    }
                }


                //大车小车设置
                if (this.Bnum > 0 && this.Bnum <= this.Count)
                {
                    prob = this.Rand.Next(100) + 1;
                    if (0 < prob && prob <= (this.PecentageBig * 100) && Bcnt < this.Bnum)
                    {
                        CarType = Definitions.BigCar;
                        Bcnt++;
                    }
                    else
                    {
                        CarType = Definitions.SmallCar;
                    }
                }

                /*车辆ID设置   车道ID + 生成时间的第一位  */
                //CarID = LaneID + "" + (int)Math.Round(gtime, 0);
                CarID = (int.Parse(LaneID) * 1000 + (int)Math.Round(gtime, 0)).ToString();


                Vehicle v;
                if (CarType == Definitions.SmallCar)
                {

                    v = new Vehicle(LaneID, DLaneID, CarID, prio, Definitions.SmallCar, gtime);
                    VehicleQueue.Enqueue(v);
                }
                else
                {
                    v = new Vehicle(LaneID, DLaneID, CarID, prio, Definitions.BigCar, gtime);
                    VehicleQueue.Enqueue(v);
                }
                index++;
            }

            //将生成车流队列Queue返回
            return VehicleQueue;
        }





        /************************************************************************/
        /* Test
        /************************************************************************/
        private Queue<Vehicle> generateVehicleQueue1(string LaneID, int Count)
        {       
                  
            //LaneID = "4010531";
            //LaneID = "4040511";
            //LaneID = SetLaneIDTest(); 

            //LaneID = SetLaneIDTest();

            Queue<Vehicle> VehicleQueue = new Queue<Vehicle>();
            int index = 0;        //车辆数目
            string DLaneID;       //标识目标车道
            int prio = -1;        //标识当前车的优先级
            int prob = -1;        //随机数的产生概率
            float ttime = 0.0f;   //原来时间
            float gtime;          //最新时间
            string CarID;         //车辆ID
            int Hcnt = 0;         //高优先级车数量
            int Mcnt = 0;         //中优先级车数量
            int Bcnt = 0;         //大车数量
            int CarType = Definitions.SmallCar; //标志车辆类型

            while (index < Count)
            {
                /*车辆生成时间*/
                gtime = this.getRandomInterval(this.Lamda);
                gtime += ttime;  //迭代产生时间 新时间
                ttime = gtime;   //迭代产生时间 保留原时间


                /*随机产生车辆的目标车道Destination_LaneID*/
                DLaneID = this.SetDstLaneID(LaneID);  //6/16


                //DLaneID = "4070341";
                //DLaneID = "4070811";
                //DLaneID = "4121311";
                //DLaneID = "4111531";
                //DLaneID = "4050141";
                //DLaneID = "4101431";


                /*优先级设置*/
                bool mark = true;      //用于标识高中优先级车辆是否超过预计数量，若超过则为true，否则为false
                while (mark)
                {
                    mark = false;
                    /*优先级位置随机生成*/
                    prob = (this.Rand.Next(1000) % MainData.SimulationConfig.GetTxtVehicleQuantity()) + 1;
                    if (prob > 50)
                    {
                        Debug.WriteLine("fdsafafs35555555");
                        Debug.WriteLine("fdsafafs111111");
                    }
                    if (0 == prob)
                    {
                        Debug.WriteLine("fdsafafs111111");
                        Debug.WriteLine("fdsafafs111111");
                    }
                    if (1 <= prob && prob <= this.Prob_High)//高优先级
                    {
                        if (Hcnt >= this.Prob_High)
                        {
                            /*超出预期的范围*/
                            Debug.WriteLine("fdsafafs33333333");
                            Debug.WriteLine("fdsafafs111111");
                            mark = true;
                            continue;
                        }
                        else
                        {
                            prio = Definitions.Prio_High;
                            Hcnt++;
                        }
                    }
                    else if (this.Prob_Mid != 0 && prob > this.Prob_High && prob <= (this.Prob_High + this.Prob_Mid))//中优先级
                    {
                        if (Mcnt >= this.Prob_Mid)
                        {
                            Debug.WriteLine("fdsafafs444444444");
                            Debug.WriteLine("fdsafafs111111");
                            mark = true;
                            continue;
                        }
                        else
                        {
                            prio = Definitions.Prio_Mid;
                            Debug.WriteLine("fdsafafs111111");
                            Mcnt++;
                        }
                    }
                    else
                    {
                        prio = Definitions.Prio_Low;
                    }
                }


                //大车小车设置
                if (this.Bnum > 0 && this.Bnum <= this.Count)
                {
                    prob = this.Rand.Next(100) + 1;
                    if (0 < prob && prob <= (this.PecentageBig * 100) && Bcnt < this.Bnum)
                    {
                        CarType = Definitions.BigCar;
                        Bcnt++;
                    }
                    else
                    {
                        CarType = Definitions.SmallCar;
                    }
                }

                /*车辆ID设置   车道ID + 生成时间的第一位  */
                //s = ((int)(LaneID * 10000 + (int)Math.Round(gtime, 0))).ToString();
                // CarID = int.Parse(LaneID)*10000000 + (int)Math.Round(gtime, 0);
                //CarID = (int.Parse(LaneID) * 1000 + (int)Math.Round(gtime, 0)).ToString();
                CarID = LaneID + (int)Math.Round(gtime, 0);

                Vehicle v;
                if (CarType == Definitions.SmallCar)
                {

                    v = new Vehicle(LaneID, DLaneID, CarID, prio, Definitions.SmallCar, gtime);
                    VehicleQueue.Enqueue(v);
                }
                else
                {
                    v = new Vehicle(LaneID, DLaneID, CarID, prio, Definitions.BigCar, gtime);
                    VehicleQueue.Enqueue(v);
                }
                index++;
            }
            //将生成车流队列Queue返回
            return VehicleQueue;

            
            
        }


        /************************************************************************/
        /* 根据输入的期望产生车头时距（1~10s）
         * 参数Lamda是单位时间间隔内的平均到达率    */
        /************************************************************************/
        private float getRandomInterval(float Lamda)
        {
            float pre = 0.0f;
            pre = (float)this.Rand.NextDouble();
            pre *= 10.0f;
            pre = (float)Math.Round(pre, Definitions.Precise);
            while (pre < Simulation.LeastInterval || pre > ((float)Math.Round(1.0f / Lamda + Simulation.LeastTimeInterval, Definitions.Precise)))   //时间范围
            {
                pre = (float)this.Rand.NextDouble();
                pre *= 10.0f;
                pre = (float)Math.Round(pre, Definitions.Precise);
            }
            while (!this.P_rand(this.Lamda, (float)Math.Round(pre, Definitions.Precise)))
            {
                pre = (float)this.Rand.NextDouble();
                pre *= 10.0f;
                pre = (float)Math.Round(pre, Definitions.Precise);
                while (pre < Simulation.LeastInterval || pre > ((float)Math.Round(1.0f / Lamda + Simulation.LeastTimeInterval, Definitions.Precise)))
                {
                    pre = (float)this.Rand.NextDouble();
                    pre *= 10.0f;
                    pre = (float)Math.Round(pre, Definitions.Precise);
                }
            }
            return pre;
        }

        /************************************************************************/
        /* 产生泊松分布随机数，根据固定数值Lamda，
         * 计算在T时间内到达出现一辆车的概率             */
        /************************************************************************/
        private bool P_rand(double Lamda, float T)
        {
            double a, b, c, d;
            double x = 1.0f;
            int p, q;
            a = Math.Pow(Lamda * T, x);
            b = Math.Exp((-1.0f) * Lamda * T);
            c = x;

            d = a * b / c;
            d *= 100;
            p = (int)d;
            q = this.Rand.Next(1000) * this.Rand.Next(1000);
            q = q % 100;

            if (q < d)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        /************************************************************************/
        /* 随机产生车辆的目的车道（DestinationLaneID）                           */
        /************************************************************************/
        private string SetDstLaneID(string Original_LaneID)
        {
            if (Original_LaneID.Equals("4161241")) 
            {
                int a = 0;
            }
            //将可以随机到达的目的车道
            string Destination_LaneID = null;
            List<Lane> LaneList = new List<Lane>(); ;
            foreach (Lane L in MainData.LeaveLaneList) 
            {
                LaneList.Add(L);
            }

            //根据走向选择不能的转向
            string temp = "";
            if(Original_LaneID[5].Equals('1'))
            {
                temp = "2";
            }
            else if(Original_LaneID[5].Equals('2'))
            {
                temp = "1";
            }
            else if (Original_LaneID[5].Equals('3'))
            {
                temp = "4";
            }
            else if (Original_LaneID[5].Equals('4'))
            {
                temp = "3";
            }
            string TempLaneID = "" + Original_LaneID[0] + Original_LaneID[3] + Original_LaneID[4] + Original_LaneID[1] + Original_LaneID[2] + temp + Original_LaneID[6];

            Lane TempLane = new Lane();
            foreach (Lane L in LaneList)
            {
                if (TempLaneID.Equals(L.GetID()))
                {
                    TempLane = L;
                }
            }
            LaneList.Remove(TempLane);  //移除不可到达的

            int LeaveLaneCount = LaneList.Count();

            int r = this.Rand.Next(LeaveLaneCount);
            Destination_LaneID = LaneList[r].GetID();

            return Destination_LaneID;
        }

        private string SetLaneIDTest()
        {
            //将可以随机到达的目的车道        
            int r = this.Rand.Next();
            if (r == 1)
            {
                return "4010531";

            }
            else {
                return "4040511";
            }

        }
        private string SetDstLaneID1(string Original_LaneID)
        {
            if (Original_LaneID.Equals("4030731"))
            {
                return "4071231";
            }
            else if (Original_LaneID.Equals("4091011"))
            {
                return "4101111";
            }
            else if (Original_LaneID.Equals("4020631"))
            {
                return "4061131"; 
            }
            else if(Original_LaneID.Equals("4030731"))
            {
                return "4071231";
            }
            else if (Original_LaneID.Equals("4010531"))
            {
                return "4051031";
                //将可以随机到达的目的车道        
                //int r = this.Rand.Next(3);
                //if (r == 0)
                //{
                //    return "4050421";
                //}
                //else if (r == 1)
                //{
                //    return "4051031";
                //}
                //else
                //{
                //    return "4050611";
                //}
            }
            else
            {
                //return "4040141";
                return "4050611";
                //将可以随机到达的目的车道        
                //int r = this.Rand.Next(3);
                //if (r == 0)
                //{
                //    return "4050141";

                //}
                //else if (r == 1)
                //{
                //    return "4051031";
                //}
                //else
                //{
                //    return "4050611";
                //}
            }            
        }


        /************************************************************************/
        /* 将车辆插入到监视区之中                                                                     */
        /************************************************************************/
        public Vehicle insertVehicle(Lane L)
        {
            //判断是否插入完，还有没有剩余车辆
            if (L.GetVehicleQueue().Count == 0)
            {
                return null;
            }

            //将Queue中车辆加入到List
            Queue<Vehicle> VehicleQueue = L.GetVehicleQueue();

            //只有该车辆的生成时间小于系统当前时间，那么该车辆可以插入
            if (VehicleQueue.Peek().Get_GenerationTime() <= Main.GetGlobalTime())
            {
                if (MainData.Map.GlobalVehicleList.Count != 0)
                {
                    Vehicle v = null;
                    foreach (Vehicle vtmp in MainData.Map.GlobalVehicleList)
                    {
                        //找到该车道上的最后一辆车
                        if ((vtmp.Get_Origin_LaneID().Equals(L.GetID()) == false) || (vtmp.Get_Current_LaneID().Equals(L.GetID()) == false)
                             || vtmp.Get_LeaveState() == Definitions.LeavePreSections || vtmp.Get_LeaveState() == Definitions.LeaveSections
                             || vtmp.Get_LeaveState() == Definitions.LeaveAfterSections)
                        {
                            continue;
                        }
                        v = vtmp; //最后一辆车赋值给V
                    }

                    if (v != null)
                    {
                        //安全距离加上车长
                        this.LeastDistance = Vehicle.MAX_SPEED_TAIL_SAFE_DISTANCE + v.CarLENGTH;

                        //离开的距离大于安全距离加车长即可发车，如果不满足条件，不会发车
                        if ((v.Get_Total_Distance() - this.LeastDistance) < 0.00000001)
                        {
                            //不能加入
                            return null;
                        }
                    }

                }

                Vehicle ve = VehicleQueue.Dequeue();
                L.SetVehicleQueue(VehicleQueue);
                MainData.UpdateLaneList(L);
                return ve; //弹出第一个元素
            }
            else
            {
                return null;
            }
        }


        /************************************************************************/
        /* 检测未插入车辆队列是否为空                                                                     */
        /************************************************************************/
        public bool isVehicleQueueEmpty()
        {
            //进入车道、中间车道标志位
            bool flagEnterLane = true;
            bool flagCenterLane = true;

            //判断所有的车道都十分有车 分为进入车道和中间车道
            foreach (Lane Lane in MainData.EnterLaneList) 
            {
                if (Lane.GetVehicleList().Count() != 0)
                {
                    flagEnterLane = false;
                }
            }

            //判断所有的车道都十分有车 分为进入车道和中间车道
            foreach(Lane Lane in MainData.CenterLaneList)
            {
                if (Lane.GetVehicleList().Count() == 0)
                {
                    flagCenterLane = false;
                }
            }

            //根据标志位，判断是否返回全部为空
            if (flagEnterLane == true && flagCenterLane == true)
            {
                return true;
            }
            else
            {
                return false;
            }
        }


    }

}
